// Code updated 5/1/07

/**************************
 * This program contains: *
 * H-Bridge control fncs  *
 **************************/
#include    <system.h>
#include	"EESD.h"
#include	"Hbridge.h"
short direction = 0; // 0 = brake, 1 = fwd, 2= rgt, 3 =lft, 4 =rvrs


// Sets H-bridge intial settings

void hbridge_init(void)
{
	unsigned short num;
	
	//*********************
	//**start of pwm code**
	//*********************
	//1. setting the pwm period by writing to the PR2 register 
	pr2 = 156;					//pwm frequency of 995 Hz
	//pr2 = 60;
	//writing to CCP1CON
	//ccp1con.5 = 0;
	//ccp1con.4 = 1;
	//5. configuring the CCPx module for PWM operation (for PWM mode: 1100)
	ccp1con.3 = 1;
	ccp1con.2 = 1;
	ccp1con.1 = 0;
	ccp1con.0 = 0;
	
	//writing to CCP2CON
	//ccp2con.5 = 0;
	//ccp2con.4 = 0;
	//5. configuring the CCPx module for PWM operation (for PWM mode: 1100 -- CCP mode, not EECP)
	ccp2con.3 = 1;
	ccp2con.2 = 1;
	ccp2con.1 = 0;
	ccp2con.0 = 0;
	
	//3. making the CCPx pin an output by clearing the appropriate tris bit
	trisc.2 = 0;	//the output of CCP1 is c2
	trisc.1 = 0;	//the output of CCP2 is c1
	
	//4. setting the TMR2 prescale value, then enable Timer2 by writing to T2CON
	//TMR2 = 16 (00 = 1; 01 = 4; 1x = 16)
	t2con.1 = 1;			//prescale value
	t2con.0 = 0;			//prescale value
	
	t2con.2 = 1;			//turning on timer2
	

	//directional variables initialization

	
	num = 0;
	ccpr1l = num >> 2;	//is this necessary? for now yes.
	ccpr2l = num >> 2;
	

	//setting the pins to be output values to PHASE and RESET
	//for Right Motor
	trisb.5 = 0;		//for PHASE
	trisc.0 = 0;		//for RESET
	//for Left Motor
	trisa.3 = 0;		//for PHASE
	trisa.2 = 0;		//for RESET
	
	//setting the pins to be input values
	trisa.4 = 0;		//for FAULT
	trisa.0 = 0;		//for FAULT
	
	
	lata.5 = 0;
	trisa.1 = 0;
	lata.1 = 0;
	
	/*if porta.0 == 1{
		lata.2 = 0;
		delay_ms(250);
		lata.2 = 1;}
	else if porta.4 == 1{
		latc.0 = 0;
		delay_ms(250);
		latc.0 = 1;}
	else {}
	*/
	latc.0 = 0;		//initialization for RESET
	lata.2 = 0;		//initialization for RESET
	
	//brake();			//initialization so reset is set 1(???)
	return;
}

/**************************
 * The following functions*
 * deal with directional  *
 * control of the HC      *
 **************************/
 

void frwd_mR (void)
{
	latb.5 = 1;		//initialization for PHASE
	lata.4 = 0;		//initialization for FAULT
	latc.0 = 1;		//initialization for RESET
}

void frwd_mL (void)
{
	lata.3 = 0;		//initialization for PHASE
	lata.0 = 1;		//initialization for FAULT
	lata.2 = 1;		//initialization for RESET
}

void rvrs_mR (void)
{
	latb.5 = 0;		//initialization for PHASE
	lata.4 = 0;		//initialization for FAULT
	latc.0 = 1;		//initialization for RESET
}

void rvrs_mL (void)
{
	lata.3 = 0;		//initialization for PHASE
	lata.0 = 0;		//initialization for FAULT
	lata.2 = 1;		//initialization for RESET
}

void brk_mR (void)
{
	latb.5 = 1;		//initialization for PHASE
	lata.4 = 0;		//initialization for FAULT
	latc.0 = 1;		//initialization for RESET
}

void brk_mL (void)
{
	lata.3 = 1;		//initialization for PHASE
	lata.0 = 0;		//initialization for FAULT
	lata.2 = 1;		//initialization for RESET
}



/*
volatile bit init_ph_L @TRISA.3;
volatile bit ph_L @LATA.3;
volatile bit init_ph_R @TRISB.5;
volatile bit ph_R @LATB.5;

volatile bit init_re_L @TRISA.2;
volatile bit re_L @LATA.2;
volatile bit init_re_R @TRISC.0;
volatile bit re_R @LATC.0;

volatile bit init_en_L @TRISC.1;
volatile bit init_en_R @TRISC.2;

volatile bit init_flt_L @TRISA.4;
volatile bit flt_L @LATA.4;
volatile bit init_flt_R @TRISA.0;
volatile bit flt_R @LATA.0;

volatile bit init_lng_L @TRISA.1;
volatile bit lng_L @LATA.1;
volatile bit init_lng_R @TRISA.5;
volatile bit lng_R @LATA.5;

short direction = 0; // 0 = brake, 1 = fwd, 2= rgt, 3 =lft, 4 =rvrs

void frwd_mL (void)
{
	ph_L = 1;
	re_L = 1;
	flt_L = 0;
}

void frwd_mR (void)
{
	ph_R = 1;
	re_R = 1;
	flt_R = 0;
}

void rvrs_mL (void)
{
	ph_L = 0;
	re_L = 1;
	flt_L = 0;
}

void rvrs_mR (void)
{
	ph_R = 0;
	re_R = 1;
	flt_R = 0;
}

void brk_mL (void)
{
	ccpr2l = 0 >> 2;	//setting dutycycle = 0
}

void brk_mR (void)
{
	ccpr1l = 0 >> 2;
}
*/


// Input is a number, output is a corresponding
// PWM setting

unsigned short pwmconvert(unsigned short in1)
{
	unsigned short num;
	/*
	if (in1 == 0)
		num = 24;	//10% dtcyc
	else if (in1 == 1)
		num = 48;	//20% dtcyc
	else if (in1 == 2)
		num = 73;	//30% dtcyc
	else if (in1 == 3)
		num = 97;	//40% dtcyc
	else if (in1 == 4)
		num = 122;	//50% dtcyc
	else if (in1 == 5)
		num = 146;	//60% dtcyc
	else if (in1 == 6)
		num	= 170;	//70% dtcyc
	else if (in1 == 7)
		num = 195;	//80% dtcyc
	else if (in1 == 8)
		num = 219;	//90% dtcyc
	else if (in1 == 9)
		num = 244;	//100% dtcyc
	return num;
	*/
	if (in1 == 0)
		num = 62;	//10% dtcyc
	else if (in1 == 1)
		num = 125;	//20% dtcyc
	else if (in1 == 2)
		num = 188;	//30% dtcyc
	else if (in1 == 3)
		num = 251;	//40% dtcyc
	else if (in1 == 4)
		num = 314;	//50% dtcyc
	else if (in1 == 5)
		num = 376;	//60% dtcyc
	else if (in1 == 6)
		num	= 439;	//70% dtcyc
	else if (in1 == 7)
		num = 502;	//80% dtcyc
	else if (in1 == 8)
		num = 565;	//90% dtcyc
	else if (in1 == 9)
		num = 628;	//100% dtcyc
	return num;
}	

// The following functions take as input the speed
// as defined in pwmconvert()


void forward (unsigned short speed)
{

	unsigned short num;

	num = pwmconvert(speed);
			
	ccpr1l = num >> 2;	
	ccpr2l = num >> 2;	

	
	frwd_mL();
	frwd_mR();
	direction = 1;
	
}

void reverse (unsigned short speed)
{

	unsigned short num;

	num = pwmconvert(speed);
			
	ccpr1l = num >> 2;	
	ccpr2l = num >> 2;	

	
	rvrs_mL();
	rvrs_mR();
	direction = 4;
	
}

void brake (void)
{
	brk_mL();
	brk_mR();
	//ccpr1l = 0 >> 2;
	//ccpr2l = 0 >> 2;
	direction = 0;
}

void turn_right (unsigned short speed)
{

	unsigned short num;
	
	num = pwmconvert(speed);
			
	ccpr1l = num >> 2;	
	ccpr2l = num >> 2;	

	frwd_mL();
	rvrs_mR();
	direction = 2;
	
}

void turn_left (unsigned short speed)
{

	unsigned short num;
	
	num = pwmconvert(speed);
			
	ccpr1l = num >> 2;	
	ccpr2l = num >> 2;	

	frwd_mR();
	rvrs_mL();
	
	direction = 3;
}


